from Drivers.AudioCapture import AudioCaptureDevice
from Algorithm import NCC,TDOA,GCC,Filter,KalmanFilter2D
import numpy as np
import time
import math
import warnings
warnings.filterwarnings('ignore')

# 周围的参数设置
SAMPLE_RATE = 48000
SAMPLE_POINTS = 16384
WAVE_SPEED = 340.0

# 打开音频设备并进行采集
dev = AudioCaptureDevice('hw:2,0','S16_LE',8,SAMPLE_RATE,2)
dev.start_capture()

def get_segment():
    p1_channel,p2_channel,p3_channel = dev.get_audio_segment(0.5,(5,6,0))
    # p1_channel = Filter.bandpass_filter(p1_channel,SAMPLE_RATE,FL,FH)
    # p2_channel = Filter.bandpass_filter(p2_channel,SAMPLE_RATE,FL,FH)
    # p3_channel = Filter.bandpass_filter(p3_channel,SAMPLE_RATE,FL,FH)
    p1_channel = p1_channel[:SAMPLE_POINTS] / 128
    p2_channel = p2_channel[:SAMPLE_POINTS] / 128
    p3_channel = p3_channel[:SAMPLE_POINTS] / 128
    return p1_channel,p2_channel,p3_channel

def get_location(p1_channel,p2_channel,p3_channel):
    # 对整个信号做切片其实是最好的办法
    p21_corr_gcc = np.abs(GCC.gcc_phat(p2_channel,p1_channel))
    p31_corr_gcc = np.abs(GCC.gcc_phat(p3_channel,p1_channel))
    p21_corr_valid = p21_corr_gcc[SAMPLE_POINTS//2-50:SAMPLE_POINTS//2+50]
    p31_corr_valid = p31_corr_gcc[SAMPLE_POINTS//2-50:SAMPLE_POINTS//2+50]
    r21,r31 = (p21_corr_valid.argmax()-50)/SAMPLE_RATE*WAVE_SPEED,(p31_corr_valid.argmax()-50)/SAMPLE_RATE*WAVE_SPEED
    # print(r21,r31)
    distance,anchor_tag = TDOA.chans_method([0,-0.15],[-0.3,0],[0.3,0],r21,r31)
    # print(anchor_tag)
    return distance,anchor_tag

def check_result_is_valid(distance,anchor_tag):
    # 检查距离与坐标是否为有效值
    # 若无效就直接跳过
    if distance <= 0.0 or np.isnan(distance): return False
    if anchor_tag[0] <= -1.6 or anchor_tag[0] >= 1.6 or np.isnan(anchor_tag[0]): return False
    if anchor_tag[1] <= 2.4 or anchor_tag[1] >= 3.1 or np.isnan(anchor_tag[1]): return False
    return True

def calc_distance(p1,p2):
    return ((p2[0]-p1[0])**2 + (p2[1]-p1[1])**2)**0.5

if __name__ == '__main__':
    kf = KalmanFilter2D.KalmanFilter2D(1,0.5)

    while True:
        ch1,ch2,ch3 = get_segment()
        # 这里的距离是由TDOA计算得到的，先不计入
        _distance,anchor_tag = get_location(ch1,ch2,ch3)
        if check_result_is_valid(_distance,anchor_tag):
            # predicted_point = kf.predict(np.array([anchor_tag]))[0]
            distance = calc_distance(anchor_tag,[0,0])
            print('距离:%0.3fm 坐标:X=%0.3f,Y=%0.3f,Theta=%0.3fdeg'%(
                distance,
                anchor_tag[0],
                anchor_tag[1],
                math.atan2(anchor_tag[1],anchor_tag[0])/(math.pi*2)*360.0
                ))
        time.sleep(0.1)
